#include <stdio.h>
#include <string.h>
#include <unistd.h>

#include <io.h>
#include <fcntl.h>
#include "system.h"
#include "protocol.h"
#include <sys/alt_timestamp.h>
#include "alt_types.h"
#include "controller/c-code/xxsubmod.h"

#define PANNING_SETPOINT 0
#define PANNING_POSITION 1
#define TILT_POSITION 2
#define U_RESET 3
#define TILT_SETPOINT 4
#define PANNING_CONTROL 0
#define TILT_CONTROL 1

#define FORWARD 0
#define BACKWARD 1
#define HORIZONTAL 1
#define VERTICAL 0
#define HOR_QUAD QUADRATURE_1_BASE
#define VER_QUAD QUADRATURE_0_BASE

void check_serial(SerialConn* conn, XXDouble u[], XXDouble y[]);
void set_pwm2(int value, char direction, char motor);
void set_pwm(int value, char motor);
void parse_packet(SerialConn* conn, XXDouble u[], XXDouble y[]);
void control_loop(SerialConn* conn);

unsigned char ACK[] = { 0x20 };
unsigned char NACK_MALFORMED[] = { 0x21 };
unsigned char NACK_CHECKSUM[] = { 0x22 };


alt_u32 start;
alt_u32 end;

/*
 Function which is used to test the serial connection.
 Receives packets and parses them
*/
void test_serial(SerialConn* conn)
{
   XXDouble u[5 + 1];
   XXDouble y[2 + 1];

   alt_timestamp_start();
   while (1)
   {
      check_serial(conn, u, y);
      //temp = alt_timestamp();
   }
}

/*
 Main loop of the program
*/
int main()
{
   printf("Hello from Nios II!\n");

   SerialConn conn;
   conn.state = START;
   conn.fd = open("/dev/uart_0", O_NONBLOCK | O_RDWR);

   if (conn.fd)
   {
      //test_serial(&conn);
      control_loop(&conn);
   }
   else
   {
      printf("Error opening serial connection\n");
   }

   return 0;
}

/* the main control loop function */
void control_loop(SerialConn* conn)
{
   XXDouble u[5 + 1];
   XXDouble y[2 + 1];

   set_pwm(0, VERTICAL);
   set_pwm(0, HORIZONTAL);

   //while (1) {}

   unsigned int d_count = 0;

   /* initialize the inputs and outputs with correct initial values */
   u[0] = 0.0; /* horizontalSetpoint */
   u[1] = 0.0; /* positionHorizontal */
   u[2] = 0.0; /* positionVertical */
   u[3] = 0.0; /* reset */
   u[4] = 0.0; /* verticalSetPoint */

   y[0] = 0.0; /* horizontalControlSignal */
   y[1] = 0.0; /* verticalControlSignal */

   /* initialize the submodel itself */
   XXInitializeSubmodel(u, y, xx_time);

   alt_timestamp_start();
   alt_u32 timing = 500000; //10ms
   alt_u32 temp;
   alt_u32 time_start = alt_timestamp();
   alt_u32 time_end = time_start;

   int last_horz = IORD(HOR_QUAD, 0x00);
   int last_vert = IORD(VER_QUAD, 0x00);
   int diff, diff2;

   /* simple loop */
   while (1)
   {
      time_end += timing;

      int encoder_vertical = IORD(VER_QUAD, 0x00);
      int encoder_horizontal = IORD(HOR_QUAD, 0x00);

      diff = 0;
      diff2 = 0;
      //Detect overflow from negative to positive
      if (encoder_vertical > 512 && last_vert < -512)
         diff = -2048;
      //Detect overflow from positive to negative
      else if (last_vert > 512 && encoder_vertical < -512) diff = 2048;

      //Detect overflow from negative to positive
      if (encoder_horizontal > 512 && last_horz < -512)
         diff2 = -2048;
      //Detect overflow from negative to positive
      else if (last_horz > 512 && encoder_horizontal < -512) diff2 = 2048;

      u[2] += (XXDouble) (encoder_vertical - last_vert + diff);
      u[1] += (XXDouble) (encoder_horizontal - last_horz + diff2);

      /* call the submodel to calculate the output */
      XXCalculateSubmodel(u, y, xx_time);

      set_pwm((int) y[1], VERTICAL);
      set_pwm((int) y[0], HORIZONTAL);


      //if (d_count % 100 == 0)
      //{
         //temp = alt_timestamp();
         //printf("%d,%f,%d,%d,%f,%d,%d\n", d_count, u[0], (int)u[1], (int)u[2], u[4], (int)y[0], (int)y[1]);
         //printf("%d,%f,%f,%f,%f,%d,%d\n", d_count, u[0], (int)u[1], (int)u[2], u[4], (int)y[0], (int)y[1]);
         //printf("us: %f %d %d %f\n", u[0], (int)u[1], (int)u[2], u[4]);
         //printf("ys: %d %d\n", (int)y[0], (int)y[1]);
         //printf("encoder: %d %d\n", last_horz, encoder_horizontal);
         //printf("state: %f\n", xx_V[66]);
         //printf("time left: %d %d %f\n", time_end - temp, time_end
         //      - alt_timestamp(), xx_time);
         //printf("PID_vert error: %f; PID_horz error: %f\n", xx_R[4], xx_R[1]);
         //printf("Horz Min: %f Max: %f\n", xx_M[0].mat[1], xx_M[0].mat[0]);
         //printf("Vert Min: %f Max: %f\n", xx_M[1].mat[1], xx_M[1].mat[0]);
      //}

      u[U_RESET] = 0.0; // set reset to zero again (can be set to 1 again by serial communication)

      temp = alt_timestamp();
      while (time_end - temp < timing) //busy wait until 10ms are over
      {
         if (time_end - temp > 50000) //if more than 1.0ms left
         {
			//check if there are any bytes to receive and parse it\
			//receive bytes one by one
            check_serial(conn, u, y);
         }
         temp = alt_timestamp();
      }
      if (temp > 2000000000) // reset timer when it gets large to avoid overflow
      {
         alt_timestamp_start();
         time_start = alt_timestamp();
         time_end = time_start;
      }
      d_count++;
      last_vert = encoder_vertical;
      last_horz = encoder_horizontal;
   }

   /* perform the final calculations */
   XXTerminateSubmodel(u, y, xx_time);

}

/*
 Sets PWM value for the specified motor
 value should be an 11 bit value including direction
 motor should be one of HORIZONTAL or VERTICAL
*/
void set_pwm(int value, char motor)
{
   unsigned int val = value & 0x7ff;
   //printf("val: %d %u\n", motor, val);
   if (motor)
   {
      IOWR(PWM_1_BASE, 0x00, val);
   }
   else
   {
      IOWR(PWM_0_BASE, 0x00, val);
   }
}

/*
 Another function to set PWM, with direction apart. Direction is one of FORWARD or BACKWARD
*/
void set_pwm2(int value, char direction, char motor)
{
   if (direction) value = ~value & 0x3ff;

   if (motor)
   {
      IOWR(PWM_1_BASE, 0x00, direction << 10 | value);
   }
   else
   {
      IOWR(PWM_0_BASE, 0x00, direction << 10 | value);
   }
}

/*
  Function to check if a byte is available on the serial port
  If it is available, parse it and process it
  The u XXDouble are inputs to the controller, y XXDouble are outputs from the controller
  Both are used by the serial communication if it receives a packet which modifies or reads these values
*/
void check_serial(SerialConn* conn, XXDouble u[], XXDouble y[])
{
   int ret = read(conn->fd, conn->rxBuffer, 1);

   if (ret != -1)
   {
      //printf("%x\n", conn->rxBuffer[0]);
      int p = parse_next(conn, conn->rxBuffer[0]);

      //if (conn->state == FRAMEDLE)
      //{
      //   start = alt_timestamp();
      //}

      if (p == READY)
      {
         //printf("packet complete\n");
         //packet complete
         parse_packet(conn, u, y);
         write_packet(conn->fd, ACK, 1);

         //end = alt_timestamp();
         //printf("%d\n", end - start);
      }
      else if (p == MALFORMED)
      {
         write_packet(conn->fd, NACK_MALFORMED, 1);
         end = alt_timestamp();
         //printf("malf %d\n", end - start);
      }
      else if (p == CHECKSUM_INCORRECT)
      {
         write_packet(conn->fd, NACK_CHECKSUM, 1);
         end = alt_timestamp();
         //printf("error %d\n", end - start);
      }
   }
}

/*
  Parses a complete packet
  This function responds to the actual messages that are sent
*/
void parse_packet(SerialConn* conn, XXDouble u[], XXDouble y[])
{
   unsigned char *p = conn->packetBuffer;
   if (p[0] == SET_HORIZONTAL)
   {
      if (conn->length == 10)
      {
         XXDouble ff;
         memcpy(&ff, p + 1, 8);
         u[PANNING_SETPOINT] = ff;
         //printf("PANNING: %f\n", u[PANNING_SETPOINT]);
      }
   }
   else if (p[0] == SET_VERTICAL)
   {
      if (conn->length == 10)
      {
         XXDouble ff;
         memcpy(&ff, p + 1, 8);
         u[TILT_SETPOINT] = ff;
         //printf("TILT: %f\n", u[TILT_SETPOINT]);
      }
   }
   else if (p[0] == SET_HORIZONTAL_DIFF)
   {
      if (conn->length == 10)
      {
         XXDouble ff;
         memcpy(&ff, p + 1, 8);
         u[PANNING_SETPOINT] += ff;
         //printf("PANNING: %f\n", u[PANNING_SETPOINT]);
      }
   }
   else if (p[0] == SET_VERTICAL_DIFF)
   {
      if (conn->length == 10)
      {
         XXDouble ff;
         memcpy(&ff, p + 1, 8);
         u[TILT_SETPOINT] += ff;
         //printf("TILT: %f\n", u[TILT_SETPOINT]);
      }
   }
   else if (p[0] == GET_POSITION)
   {
      if (conn->length == 2)
      {
         unsigned char out[17];
         out[0] = GET_POSITION;
         memcpy(out + 1, &(u[1]), 16);
         write_packet(conn->fd, out, 17);
         //printf("GETPOSITION: %f %f\n", u[1], u[2]);
      }
   }
   else if (p[0] == RESET)
   {
      if (conn->length == 2)
      {
         u[U_RESET] = 1.0;
         //printf("RESETTING...\n");
      }
   }
}
